/*----------------------------------------------------------------------------*/
/* Copyright (c) FIRST 2008. All Rights Reserved.							  */
/* Open Source Software - may be modified and shared by FRC teams. The code   */
/* must be accompanied by the FIRST BSD license file in $(WIND_BASE)/WPILib.  */
/*----------------------------------------------------------------------------*/

#include "Gyro.h"
#include "AnalogChannel.h"
#include "AnalogModule.h"
#include "Timer.h"
#include "Utility.h"


class smartGyro{
	Gyro tilter;
	bool isOn;
	bool hasStarted ;
	bool firstRun ;
	int counter ; 
int window ;
double multiplyer;

/**
 * Initialize the gyro.
 * Calibrate the gyro by running for a number of samples and computing the center value for this
 * part. Then use the center value as the Accumulator center value for subsequent measurements.
 * It's important to make sure that the robot is not moving while the centering calculations are
 * in progress, this is typically done when the robot is first turned on while it's sitting at
 * rest before the competition starts.
 */
public:

	
	 smartGyro():
		tilter(1)
	
	{
		 hasStarted = false;
		 window = 25;
		 multiplyer = 1;
		
	
		
	}
	 smartGyro(double mul):
	 		tilter(1)
	 	
	 	{
		 multiplyer = mul ;
		 if (multiplyer > 1)
			 multiplyer = 1;
		 if (multiplyer < -1)
			 multiplyer = -1;
				 
		 
		 
	 	}
	 
	 
	float smartGyro::GetSpeed( void )
	{
		if (firstRun){
			//tilter.Reset(); // placeholder
			firstRun = false ;
			printf("gyro first reset \n");
			
		}
		
		float out = 0 ;
		float angle = tilter.GetAngle();
		if (angle > 5 && angle < 180 )
		{hasStarted = true;}
		else if( angle > 300 && angle < 360)
		{hasStarted = true;}
		if (!hasStarted){
			
		//	out = 1.0;
			//hasStarted = true
		}
		else{
			if (angle >= 4  )
				out =(( angle / window) * multiplyer)+.15; //placeholder
			else if (angle < - 5 )
				out =(( angle / window) * multiplyer)+.15 ; //placeholder
			
				
		}
	
		if (counter < 100)
			counter = counter + 1;
		else if (counter >= 100)
			printf(   "Vert Angle = %f\n",tilter.GetAngle());
		printf(   "motor now = %f\n",out);
		/*if (out > 1)
					 out = 1;
				 if (out < -1)
					 out = -1;
			
		*/
		
		return out ;
	}
/*	float smartGyro::GetShifter( void )
	{
		if (firstRun){
			tilter.Reset();
			firstRun = false ;
			
		}
		float out = 0 ;
		int angle = tilter.GetAngle();

		
			if (angle >= 4 && angle < window + 5 )
				out = angle / window;//placeholder
			else if (angle > window - 5 && angle < 355)
				out =  (angle - (360 - window)) - window / window;//placeholder
			
				
		
			 
		
		return out ;
	}*/
	void smartGyro::Reset( void )
		{
		tilter.Reset();
		printf("gyro manual reset \n");
		printf("gyro manual reset \n");

		}

	
	
	
	

};
	
